#include "FnVolumeHarmonicParam.h"
#include "FnSurfaceHarmonicParam.h"
#include <cmath>
#include "./taucs/taucs_fix.h"


FnVolumeHarmonicParam::FnVolumeHarmonicParam(Mesh& _mesh)
  : mesh_(_mesh), weight_type_(kUniform)
{
  mesh().add_property(vprop_lns_idx_, "vprop_lns_idx");  

  mesh().add_property(vprop_param_, "vprop_param");
  mesh().add_property(eprop_volume_w_, "eprop_volume_w");
  mesh().add_property(eprop_surface_w_, "eprop_surface_w");
  mesh().add_property(fprop_side_idx_, "fprop_side_idx");
  mesh().add_property(vprop_type_, "vprop_type");
  mesh().add_property(eprop_type_, "eprop_type");
  
}

FnVolumeHarmonicParam::~FnVolumeHarmonicParam(void)
{
  mesh().remove_property(vprop_lns_idx_);

  mesh().remove_property(vprop_param_);
  mesh().remove_property(eprop_volume_w_);
  mesh().remove_property(eprop_surface_w_);
  mesh().remove_property(fprop_side_idx_);
  mesh().remove_property(vprop_type_);
  mesh().remove_property(eprop_type_);
 
}


void FnVolumeHarmonicParam::print_info()
{
  std::cout << "weight_type: ";
  if (weight_type() == kUniform)            std::cout << "uniform edge weight." << std::endl;
  else if (weight_type() == kCotangent)     std::cout << "cotangent edge weight." << std::endl;
}




void FnVolumeHarmonicParam::set_cube_corner_vertices(const std::vector<Mesh::VertexHandle>& _corner_vertices,
                                                     const std::vector<Param>& _corner_params)
{
  assert(_corner_vertices.size() == 8 && _corner_params.size() == 8);

  Mesh::VertexIter    v_it;
  std::size_t   i;

  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
    if (mesh().property(vprop_type_, v_it) == kCornerVertex)
      mesh().property(vprop_type_, v_it) = kTypelessVertex;
  

  for (i=0; i<8; ++i)
  {
    Mesh::VertexHandle  vh  = _corner_vertices[i];
    Param               uvw = _corner_params[i];

    if (mesh().property(vprop_type_, vh) != kCornerVertex)
    {
      std::cerr << vh << " is not tagged as a corner vertex" << std::endl;
      return;
    }

    mesh().property(vprop_param_, vh) = uvw;
  }
}

void FnVolumeHarmonicParam::get_cube_corner_vertices(std::vector<Mesh::VertexHandle>& _corner_vertices,
                                                     std::vector<Param>& _corner_params)
{
  _corner_vertices.clear();
  _corner_params.clear();


  Mesh::VertexIter    v_it;

  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    if (mesh().property(vprop_type_, v_it) != kCornerVertex)
      continue;

    _corner_vertices.push_back(v_it);
    _corner_params.push_back(mesh().property(vprop_param_, v_it));
  }
}




void FnVolumeHarmonicParam::init()
{
  Mesh::VertexIter      v_it;
  Mesh::VertexEdgeIter  ve_it;  
  Mesh::EdgeIter        e_it;
  Mesh::EdgeFaceIter    ef_it;  
  int                   lns_idx;

  // set eprop_type_
  for (e_it=mesh().edges_begin(); e_it!=mesh().edges_end(); ++e_it)
  {
    mesh().property(eprop_type_, e_it) = kTypelessEdge;
    
    std::set<int>   side_idx_set;
    for (ef_it=mesh().ef_iter(e_it); ef_it; ++ef_it)
    {
      int side_idx = mesh().property(fprop_side_idx_, ef_it);
      if (side_idx >= 0)
        side_idx_set.insert(side_idx);
    }

    if (side_idx_set.size() == 1)
      mesh().property(eprop_type_, e_it) = kSideEdge;
    else if (side_idx_set.size() >= 2)
      mesh().property(eprop_type_, e_it) = kWedgeEdge;
  }

  // set eprop_volume_w_ & eprop_surface_w_
  update_edge_weights();

  // set vprop_type_ & vprop_lns_idx_
  lns_idx = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    int         num_adj_edge_on_wedge = 0;
    int         num_adj_edge_on_side = 0;
    VertexType  vtype = kTypelessVertex;

    for (ve_it=mesh().ve_iter(v_it); ve_it; ++ve_it)
    {
      if (mesh().property(eprop_type_, ve_it) == kSideEdge)         ++num_adj_edge_on_side;
      else if (mesh().property(eprop_type_, ve_it) == kWedgeEdge)   ++num_adj_edge_on_wedge;
    }

    if (num_adj_edge_on_wedge > 2)          vtype = kCornerVertex;
    else if (num_adj_edge_on_wedge == 2)    vtype = kWedgeVertex;
    else if (num_adj_edge_on_side > 0)      vtype = kSideVertex;

    mesh().property(vprop_type_, v_it) = vtype;
    mesh().property(vprop_lns_idx_, v_it) = (vtype == kTypelessVertex) ? lns_idx++ : -1;
  }
}

void FnVolumeHarmonicParam::calc()
{
  calc_param_on_wedges();
  calc_param_on_sides();

  calc_with_taucs();
}


void FnVolumeHarmonicParam::update_edge_weights()
{
  Mesh::EdgeIter        e_it;
  Mesh::EdgeFaceIter    ef_it;
  Scalar                volume_w;
  Scalar                surface_w;

  for (e_it=mesh().edges_begin(); e_it!=mesh().edges_end(); ++e_it)
  {
    volume_w = 0;
    if (weight_type_ == kUniform)         volume_w = 1;
    else if (weight_type_ == kCotangent)  volume_w = cot_volume_weight(e_it);

    // handling the boundary cases
    if (mesh().is_boundary(e_it))
      volume_w *= 0.5;


    surface_w = 0;    
    if (mesh().property(eprop_type_, e_it) == kSideEdge)
    {
      if (weight_type_ == kUniform)         surface_w = 1;
      else if (weight_type_ == kCotangent)  surface_w = cot_surface_weight(e_it);
    }
    
    mesh().property(eprop_volume_w_, e_it) = volume_w;
    mesh().property(eprop_surface_w_, e_it) = surface_w;
  }
}


Scalar FnVolumeHarmonicParam::cot_triangle(const Mesh::HalfedgeHandle _corner) const
{
  Mesh::Point         p, q, r;
  OpenVolMesh::Vec3d  u, v;
  double              dot_val;

  p = mesh().point(mesh().to_vertex_handle(_corner));
  q = mesh().point(mesh().to_vertex_handle(mesh().next_halfedge_handle(_corner)));
  r = mesh().point(mesh().from_vertex_handle(_corner));

  u = OpenVolMesh::Vec3d(q[0]-p[0], q[1]-p[1], q[2]-p[2]);
  v = OpenVolMesh::Vec3d(r[0]-p[0], r[1]-p[1], r[2]-p[2]);

  dot_val = OpenVolMesh::dot(u, v);
  
  return  (Scalar) (dot_val / std::sqrt(u.sqrnorm()*v.sqrnorm() - dot_val*dot_val));
}


Scalar FnVolumeHarmonicParam::cot_tetrahedron(const Mesh::HalfedgeHandle _corner) const
{
  Mesh::HalfedgeHandle  heh;
  Mesh::HalffaceHandle  hfh[2];
  OpenVolMesh::Vec3d    u, v;
  double                dot_val;

  hfh[0] = mesh().halfface_handle(_corner);
  hfh[1] = mesh().opposite_halfface_handle(mesh().halfface_handle(mesh().opposite_halfedge_handle(_corner)));

  u = OpenVolMesh::vector_cast<OpenVolMesh::Vec3d, Mesh::Normal> (mesh().calc_halfface_normal(hfh[0]));
  v = OpenVolMesh::vector_cast<OpenVolMesh::Vec3d, Mesh::Normal> (mesh().calc_halfface_normal(hfh[1]));

  dot_val = OpenVolMesh::dot(u, v);
  
  return  (Scalar) (dot_val / std::sqrt(u.sqrnorm()*v.sqrnorm() - dot_val*dot_val));
}



Scalar FnVolumeHarmonicParam::cot_surface_weight(const Mesh::EdgeHandle _eh) const
{
  if (mesh().property(eprop_type_, _eh) != kSideEdge)
    return  0;

  Mesh::ConstHalfedgeHalfedgeIter   chehe_it;
  Mesh::FaceHandle                  fh;
  Scalar                            w;

  w = 0;
  for (chehe_it=mesh().chehe_iter(mesh().halfedge_handle(_eh)); chehe_it; ++chehe_it)
  {
    fh = mesh().face_handle(chehe_it);
    if (mesh().property(fprop_side_idx_, fh) < 0)
      continue;

    w += cot_triangle(mesh().next_halfedge_handle(chehe_it));
  }

  return  w;
}


Scalar FnVolumeHarmonicParam::cot_volume_weight(const Mesh::EdgeHandle _eh) const
{
  if (mesh().property(eprop_type_, _eh) != kTypelessEdge)
    return  0;

  Mesh::ConstHalfedgeHalfedgeIter   chehe_it;
  Mesh::HalfedgeHandle              heh;
  Mesh::HalffaceHandle              hfh;
  OpenVolMesh::Vec4d                u, v;
  Mesh::Normal                      n[2];
  Scalar                            w;

  w = 0;
  for (chehe_it=mesh().chehe_iter(mesh().halfedge_handle(_eh)); chehe_it; ++chehe_it)
  {
    heh = mesh().next_halfedge_handle(mesh().opposite_halfedge_handle(mesh().prev_halfedge_handle(chehe_it)));
    w += cot_tetrahedron(heh);
  }

  return  w;
}


Mesh::HalfedgeHandle FnVolumeHarmonicParam::next_halfedge_handle_on_wedge(const Mesh::HalfedgeHandle _heh) const
{
  Mesh::VertexHandle              from_vh, to_vh, nvh;
  Mesh::EdgeHandle                eh;
  Mesh::ConstVertexOHalfedgeIter  cvohe_it;

  from_vh = mesh().from_vertex_handle(_heh);
  to_vh = mesh().to_vertex_handle(_heh);

  for (cvohe_it=mesh().cvohe_iter(to_vh); cvohe_it; ++cvohe_it)
  {
    eh = mesh().edge_handle(cvohe_it);
    if (mesh().property(eprop_type_, eh) != kWedgeEdge)
      continue;

    nvh = mesh().to_vertex_handle(cvohe_it);
    if (nvh != from_vh)
      return  cvohe_it;
  }

  return  Mesh::InvalidHalfedgeHandle;
}


Mesh::HalfedgeHandle FnVolumeHarmonicParam::prev_halfedge_handle_on_wedge(const Mesh::HalfedgeHandle _heh) const
{
  Mesh::VertexHandle              from_vh, to_vh, nvh;
  Mesh::EdgeHandle                eh;
  Mesh::ConstVertexIHalfedgeIter  cvihe_it;

  from_vh = mesh().from_vertex_handle(_heh);
  to_vh = mesh().to_vertex_handle(_heh);

  for (cvihe_it=mesh().cvihe_iter(from_vh); cvihe_it; ++cvihe_it)
  {
    eh = mesh().edge_handle(cvihe_it);
    if (mesh().property(eprop_type_, eh) != kWedgeEdge)
      continue;

    nvh = mesh().from_vertex_handle(cvihe_it);
    if (nvh != to_vh)
      return  cvihe_it;
  }

  return  Mesh::InvalidHalfedgeHandle;
}


Mesh::HalfedgeHandle FnVolumeHarmonicParam::find_first_halfedge_handle_on_wedge(const Mesh::HalfedgeHandle _heh) const
{
  Mesh::HalfedgeHandle  first_heh;
  Mesh::VertexHandle    from_vh;

  first_heh = _heh;
  from_vh = mesh().from_vertex_handle(first_heh);
  while (mesh().property(vprop_type_, from_vh) != kCornerVertex)
  {
    first_heh = prev_halfedge_handle_on_wedge(first_heh);
    from_vh = mesh().from_vertex_handle(first_heh);
  }

  return  first_heh;
}


void FnVolumeHarmonicParam::calc_param_on_wedges()
{
  Mesh::EdgeIter          e_it;
  Mesh::EdgeHandle        eh;
  Mesh::HalfedgeHandle    heh, first_heh;
  OpenVolMesh::EPropHandleT<bool>   eprop_visit_flag;


  mesh().add_property(eprop_visit_flag);
  for (e_it=mesh().edges_begin(); e_it!=mesh().edges_end(); ++e_it)
    mesh().property(eprop_visit_flag, e_it) = false;

  for (e_it=mesh().edges_begin(); e_it!=mesh().edges_end(); ++e_it)
  {
    if (mesh().property(eprop_type_, e_it) != kWedgeEdge)
      continue;
    if (mesh().property(eprop_visit_flag, e_it) == true)
      continue;

    std::vector<Mesh::VertexHandle>   vertices_on_wedge;
    std::vector<Scalar>               acc_lens;
    Scalar                            total_len;
    Mesh::VertexHandle                from_vh, to_vh;
    Param                             corner_uvw[2];

    heh = find_first_halfedge_handle_on_wedge(mesh().halfedge_handle(e_it));
    from_vh = mesh().from_vertex_handle(heh);
    vertices_on_wedge.push_back(from_vh);
    acc_lens.push_back(0);

    while (true)
    {
      eh = mesh().edge_handle(heh);      
      mesh().property(eprop_visit_flag, eh) = true;

      to_vh = mesh().to_vertex_handle(heh);      
      vertices_on_wedge.push_back(to_vh);
      acc_lens.push_back(acc_lens.back() + mesh().calc_edge_length(eh));

      if (mesh().property(vprop_type_, to_vh) == kCornerVertex)
        break;

      heh = next_halfedge_handle_on_wedge(heh);
    }
    assert(vertices_on_wedge.size() >= 2);
    assert(mesh().property(vprop_type_, vertices_on_wedge.front()) == kCornerVertex);
    assert(mesh().property(vprop_type_, vertices_on_wedge.back()) == kCornerVertex);

    corner_uvw[0] = mesh().property(vprop_param_, vertices_on_wedge.front());
    corner_uvw[1] = mesh().property(vprop_param_, vertices_on_wedge.back());
    total_len = acc_lens.back();
    std::cout << "------------------" << std::endl;
    for (std::size_t i=0; i<vertices_on_wedge.size(); ++i)
    {
      mesh().property(vprop_param_, vertices_on_wedge[i]) 
        = corner_uvw[0] + (acc_lens[i]/total_len)*(corner_uvw[1] - corner_uvw[0]);

      std::cout << "pos: " << mesh().point(vertices_on_wedge[i]) 
                << ", uvw: " << mesh().property(vprop_param_, vertices_on_wedge[i])  << std::endl;
    }
    std::cout << "------------------" << std::endl;
  }
  mesh().remove_property(eprop_visit_flag);
}



void FnVolumeHarmonicParam::calc_param_on_sides()
{
  Mesh::FaceIter  f_it;
  int             side_idx;
  std::set<int>   set_side_idx;
  std::set<int>::iterator   it;
  

  for (f_it=mesh().faces_begin(); f_it!=mesh().faces_end(); ++f_it)
  {
    side_idx = mesh().property(fprop_side_idx_, f_it);
    if (side_idx < 0)
      continue;

    set_side_idx.insert(side_idx);
  }

  for (it=set_side_idx.begin(); it!=set_side_idx.end(); ++it)
  {
    side_idx = *it;

    FnSurfaceHarmonicParam    fnSurfaceParam(mesh());

    fnSurfaceParam.set_weight_type(weight_type());
    fnSurfaceParam.set_side_index(side_idx);
    fnSurfaceParam.init();
    fnSurfaceParam.calc();
  }    
}


void FnVolumeHarmonicParam::read_face_side_info(const std::string& _filename)
{
  std::vector<std::string>  face_group_names;
  std::size_t   i;
  FILE          *fp;

  fp = fopen(_filename.c_str(), "r");

  char    s[1024];
  int     group_idx = -1;
  int     face_idx = -1;

  memset(&s, 0, 1024);
  
  while(fp && !feof(fp) && fgets(s, 1024, fp))
  {     
    // skip comment
    if (s[0] == '#' || isspace(s[0])) continue;   

    // group
    else if (strncmp(s, "g ", 2) == 0)
    {
      char  buffer[512];

      memset(&buffer, 0, 512);
      sscanf(s, "g %s", buffer);
      
      std::string   name(buffer);
      if (name == "<noname>")
      {
        group_idx = -1;
      }
      else
      {
        group_idx = -1;
        for (i=0; i<face_group_names.size(); ++i)
        {
          if (name == face_group_names[i])
          {
            group_idx = (int) i;
            break;
          }
        }
        if (group_idx < 0)
        {
          group_idx = (int) face_group_names.size();
          face_group_names.push_back(name);
        }
      }
      //std::cout << "group " << name << " " << group_idx << std::endl;
    }
    // face
    else if (strncmp(s, "f ", 2) == 0)
    {      
      Mesh::FaceHandle    fh;

      ++face_idx;
      //std::cout << face_idx << " " << group_idx << std::endl;

      fh = mesh().face_handle(face_idx);
      mesh().property(fprop_side_idx_, fh) = group_idx;
    }

    memset(&s, 0, 200);
  }
  fclose(fp);

  nSides = face_group_names.size();

  Mesh::VertexIter      v_it;
  Mesh::VertexFaceIter  vf_it;
  int                   face_side_idx;
  Param                 uvw;

  std::cout << "corner vertex infos" << std::endl;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
	  std::set<int>   adj_face_side_idx;
	  std::set<int>::const_iterator pos;
	  for (vf_it=mesh().vf_iter(v_it); vf_it; ++vf_it)
	  {
		  face_side_idx = mesh().property(fprop_side_idx_, vf_it);
		  if (face_side_idx < 0)
			  continue;
		  adj_face_side_idx.insert(face_side_idx);
	  }


	  if (adj_face_side_idx.size() < 3)
		  continue;

	  int maxlevel = (nSides-2)/4;
	  float umax = 1.f/maxlevel;
	  float vmax = umax;
	  //3 neighbor sides
	  if (adj_face_side_idx.size() == 3)
	  {
		  int l = 0;
		  int w = 0;
		  if (adj_face_side_idx.find(1) != adj_face_side_idx.end())			  //side 1
		  {
			  l = maxlevel-1;
			  w = 1;
		  }
		  if (adj_face_side_idx.find(l*4 + 2) != adj_face_side_idx.end() &&
		  adj_face_side_idx.find(l*4 + 3) != adj_face_side_idx.end())
		  {
			  uvw = Param(0,vmax,w);
		  }
		  else if (adj_face_side_idx.find(l*4 + 3) != adj_face_side_idx.end() &&
			  adj_face_side_idx.find(l*4 + 4) != adj_face_side_idx.end())
		  {
			  uvw = Param(umax,vmax,w);
		  }
		  else if (adj_face_side_idx.find(l*4 + 4) != adj_face_side_idx.end() &&
			  adj_face_side_idx.find(l*4 + 5) != adj_face_side_idx.end())
		  {
			  uvw = Param(umax,0,w);
		  }
		  else if (adj_face_side_idx.find(l*4 + 5) != adj_face_side_idx.end() &&
			  adj_face_side_idx.find(l*4 + 2) != adj_face_side_idx.end())
		  {
			  uvw = Param(0,0,w);
		  }
	  }
	  else 	  //4 neighbor patch
	  {
		  for (int i = 1; i < maxlevel; i++)
		  {
			  bool flag  = false;
			  for(pos = adj_face_side_idx.begin(); pos != adj_face_side_idx.end(); ++pos)
			  {
				  int j = i-1;
				  if (*pos/6 == j)
				  {
					  if (adj_face_side_idx.find(j*4 + 2) != adj_face_side_idx.end() &&
						  adj_face_side_idx.find(j*4 + 3) != adj_face_side_idx.end())
					  {
						  uvw = Param(0,vmax,i*umax);
					  }
					  else if (adj_face_side_idx.find(j*4 + 3) != adj_face_side_idx.end() &&
						  adj_face_side_idx.find(j*4 + 4) != adj_face_side_idx.end())
					  {
						  uvw = Param(umax,vmax,i*umax);
					  }
					  else if (adj_face_side_idx.find(j*4 + 4) != adj_face_side_idx.end() &&
						  adj_face_side_idx.find(j*4 + 5) != adj_face_side_idx.end())
					  {
						  uvw = Param(umax,0,i*umax);
					  }
					  else if (adj_face_side_idx.find(j*4 + 5) != adj_face_side_idx.end() &&
						  adj_face_side_idx.find(j*4 + 2) != adj_face_side_idx.end())
					  {
						  uvw = Param(0,0,i*umax);
					  }
					  flag = true;
					  break;
				  }
			  }
			  if (flag)
				  break;
		  }
	  }

	  mesh().property(vprop_type_, v_it) = kCornerVertex;
	  mesh().property(vprop_param_, v_it) = uvw;

	  std::cout << v_it << std::endl;
	  std::cout << "   is_boudary: " << (mesh().is_boundary(v_it) ? "true" : "false") << std::endl;
	  std::cout << "   pos: " << mesh().point(v_it) << std::endl;
	  std::cout << "   uvw: " << mesh().property(vprop_param_, v_it) << std::endl;
  }
}


//
//
//void FnVolumeHarmonicParam::read_face_side_info(const std::string& _filename)
//{
//  std::vector<std::string>  face_group_names;
//  std::size_t   i;
//  FILE          *fp;
//
//  fp = fopen(_filename.c_str(), "r");
//
//  char    s[1024];
//  int     group_idx = -1;
//  int     face_idx = -1;
//
//  memset(&s, 0, 1024);
//  
//  while(fp && !feof(fp) && fgets(s, 1024, fp))
//  {     
//    // skip comment
//    if (s[0] == '#' || isspace(s[0])) continue;   
//
//    // group
//    else if (strncmp(s, "usemtl ", 7) == 0)
//    {
//      char  buffer[512];
//
//      memset(&buffer, 0, 512);
//      sscanf(s, "usemtl %s", buffer);
//      
//      std::string   name(buffer);
//      if (name == "<noname>")
//      {
//        group_idx = -1;
//      }
//      else
//      {
//        group_idx = -1;
//        for (i=0; i<face_group_names.size(); ++i)
//        {
//          if (name == face_group_names[i])
//          {
//            group_idx = (int) i;
//            break;
//          }
//        }
//        if (group_idx < 0)
//        {
//          group_idx = (int) face_group_names.size();
//          face_group_names.push_back(name);
//        }
//      }
//      //std::cout << "group " << name << " " << group_idx << std::endl;
//    }
//    // face
//    else if (strncmp(s, "f ", 2) == 0)
//    {      
//      Mesh::FaceHandle    fh;
//
//      ++face_idx;
//      //std::cout << face_idx << " " << group_idx << std::endl;
//
//      fh = mesh().face_handle(face_idx);
//      mesh().property(fprop_side_idx_, fh) = group_idx;
//    }
//
//    memset(&s, 0, 200);
//  }
//  fclose(fp);
//
//
//  // automatically find corner vertices by using group name
//  int   nx_face_side_idx = -1, px_face_side_idx = -1;
//  int   ny_face_side_idx = -1, py_face_side_idx = -1;
//  int   nz_face_side_idx = -1, pz_face_side_idx = -1;
//  for (i=0; i<face_group_names.size(); ++i)
//  {
//    std::string& group_name = face_group_names[i];
//
//    if (group_name == "-x")         nx_face_side_idx = (int) i;
//    else if (group_name == "+x")    px_face_side_idx = (int) i;
//    else if (group_name == "-y")    ny_face_side_idx = (int) i;
//    else if (group_name == "+y")    py_face_side_idx = (int) i;
//    else if (group_name == "-z")    nz_face_side_idx = (int) i;
//    else if (group_name == "+z")    pz_face_side_idx = (int) i;
//  }
//
//  if (nx_face_side_idx < 0 || px_face_side_idx < 0 ||
//      ny_face_side_idx < 0 || py_face_side_idx < 0 ||
//      nz_face_side_idx < 0 || pz_face_side_idx < 0)
//    return;
//
//  std::cout << "nx_face_side_idx: " << nx_face_side_idx << std::endl;
//  std::cout << "px_face_side_idx: " << px_face_side_idx << std::endl;
//  std::cout << "ny_face_side_idx: " << ny_face_side_idx << std::endl;
//  std::cout << "py_face_side_idx: " << py_face_side_idx << std::endl;
//  std::cout << "nz_face_side_idx: " << nz_face_side_idx << std::endl;
//  std::cout << "pz_face_side_idx: " << pz_face_side_idx << std::endl;
//
//  FnSurfaceHarmonicParam::nx_face_side_idx = nx_face_side_idx;
//  FnSurfaceHarmonicParam::px_face_side_idx = px_face_side_idx;
//  FnSurfaceHarmonicParam::ny_face_side_idx = ny_face_side_idx;
//  FnSurfaceHarmonicParam::py_face_side_idx = py_face_side_idx;
//  FnSurfaceHarmonicParam::nz_face_side_idx = nz_face_side_idx;
//  FnSurfaceHarmonicParam::pz_face_side_idx = pz_face_side_idx;
//
//
//  Mesh::VertexIter      v_it;
//  Mesh::VertexFaceIter  vf_it;
//  int                   face_side_idx;
//  Param                 uvw;
//
//  std::cout << "corner vertex infos" << std::endl;
//  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
//  {
//    std::set<int>   adj_face_side_idx;
//    for (vf_it=mesh().vf_iter(v_it); vf_it; ++vf_it)
//    {
//      face_side_idx = mesh().property(fprop_side_idx_, vf_it);
//      if (face_side_idx < 0)
//        continue;
//      adj_face_side_idx.insert(face_side_idx);
//    }
//
//    if (adj_face_side_idx.size() != 3)
//      continue;
//
//    if (adj_face_side_idx.find(nx_face_side_idx) != adj_face_side_idx.end() &&          // -x
//        adj_face_side_idx.find(ny_face_side_idx) != adj_face_side_idx.end() &&          // -y
//        adj_face_side_idx.find(nz_face_side_idx) != adj_face_side_idx.end())            // -z
//      uvw = Param(0,0,0);
//    else if (adj_face_side_idx.find(px_face_side_idx) != adj_face_side_idx.end() &&     // +x
//             adj_face_side_idx.find(ny_face_side_idx) != adj_face_side_idx.end() &&     // -y
//             adj_face_side_idx.find(nz_face_side_idx) != adj_face_side_idx.end())       // -z
//      uvw = Param(1,0,0);
//    else if (adj_face_side_idx.find(px_face_side_idx) != adj_face_side_idx.end() &&     // +x
//             adj_face_side_idx.find(py_face_side_idx) != adj_face_side_idx.end() &&     // +y
//             adj_face_side_idx.find(nz_face_side_idx) != adj_face_side_idx.end())       // -z
//      uvw = Param(1,1,0);
//    else if (adj_face_side_idx.find(nx_face_side_idx) != adj_face_side_idx.end() &&     // -x
//             adj_face_side_idx.find(py_face_side_idx) != adj_face_side_idx.end() &&     // +y
//             adj_face_side_idx.find(nz_face_side_idx) != adj_face_side_idx.end())       // -z
//      uvw = Param(0,1,0);
//    else if (adj_face_side_idx.find(nx_face_side_idx) != adj_face_side_idx.end() &&     // -x
//             adj_face_side_idx.find(ny_face_side_idx) != adj_face_side_idx.end() &&     // -y
//             adj_face_side_idx.find(pz_face_side_idx) != adj_face_side_idx.end())       // +z
//      uvw = Param(0,0,1);
//    else if (adj_face_side_idx.find(px_face_side_idx) != adj_face_side_idx.end() &&     // +x
//             adj_face_side_idx.find(ny_face_side_idx) != adj_face_side_idx.end() &&     // -y
//             adj_face_side_idx.find(pz_face_side_idx) != adj_face_side_idx.end())       // +z
//      uvw = Param(1,0,1);
//    else if (adj_face_side_idx.find(px_face_side_idx) != adj_face_side_idx.end() &&     // +x
//             adj_face_side_idx.find(py_face_side_idx) != adj_face_side_idx.end() &&     // +y
//             adj_face_side_idx.find(pz_face_side_idx) != adj_face_side_idx.end())       // +z
//      uvw = Param(1,1,1);
//    else if (adj_face_side_idx.find(nx_face_side_idx) != adj_face_side_idx.end() &&     // -x
//             adj_face_side_idx.find(py_face_side_idx) != adj_face_side_idx.end() &&     // +y
//             adj_face_side_idx.find(pz_face_side_idx) != adj_face_side_idx.end())       // +z
//      uvw = Param(0,1,1);
//
//    mesh().property(vprop_type_, v_it) = kCornerVertex;
//    mesh().property(vprop_param_, v_it) = uvw;
//
//    std::cout << v_it << std::endl;
//	std::cout << "   is_boudary: " << (mesh().is_boundary(v_it) ? "true" : "false") << std::endl;
//    std::cout << "   pos: " << mesh().point(v_it) << std::endl;
//    std::cout << "   uvw: " << mesh().property(vprop_param_, v_it) << std::endl;
//  }
//}
//
//


void FnVolumeHarmonicParam::debug_output_params()
{
  Mesh::VertexIter    v_it;
  
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    std::cout << "vtx: " << v_it.handle().idx() << "     pos: " << mesh().point(v_it) << ", uvw: " << mesh().property(vprop_param_, v_it) << std::endl;
  }
}

void FnVolumeHarmonicParam::debug_save_boundary(const std::string& _filename)
{
  FILE    *fp;

  fp = fopen(_filename.c_str(), "w");

  Mesh::VertexIter            v_it;
  Mesh::HalffaceIter          hf_it;
  Mesh::HalffaceVertexIter    hfv_it;
  Mesh::Point                 p;
  OpenVolMesh::VPropHandleT<int>    vprop_new_idx;
  int   vidx;

  mesh().add_property(vprop_new_idx);

  
  vidx=0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    if (!mesh().is_boundary(v_it))
      continue;
    mesh().property(vprop_new_idx, v_it) = vidx++;
  }
  std::cout << "vidx: " << vidx << std::endl;
    

  // v
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    vidx = mesh().property(vprop_new_idx, v_it);
    if (vidx < 0)
      continue;

    p = mesh().point(v_it);
    fprintf(fp, "v %f %f %f\n", p[0], p[1], p[2]);
  }


  // f
  for (hf_it=mesh().halffaces_begin(); hf_it!=mesh().halffaces_end(); ++hf_it)
  {
    if (!mesh().is_boundary(hf_it))
      continue;
  
    fprintf(fp, "f");
    for (hfv_it=mesh().hfv_iter(hf_it); hfv_it; ++hfv_it)
    {
      vidx = mesh().property(vprop_new_idx, hfv_it);
      fprintf(fp, " %d", 1+vidx);
    }
    fprintf(fp, "\n");
  }

  mesh().remove_property(vprop_new_idx);


  fclose(fp);
}

// assumption: tetrahedron
void FnVolumeHarmonicParam::debug_param_flipping_test() const
{
  Mesh::ConstCellIter   cc_it;
  Mesh::HalfedgeHandle  heh;
  Mesh::VertexHandle    vh[4];
  Param                 uvw[4];
  Mesh::Point           pos[4];
  Scalar                sdv, sdpv;

  std::cout << "**************************************" << std::endl;
  std::cout << "    Flipping Test " << std::endl;
  for (cc_it=mesh().cells_begin(); cc_it!=mesh().cells_end(); ++cc_it)
  {
    heh = mesh().chfhe_iter(mesh().ccihf_iter(cc_it));
    vh[0] = mesh().to_vertex_handle(heh);
    uvw[0] = mesh().property(vprop_param_, vh[0]);
    pos[0] = mesh().point(vh[0]);
    heh = mesh().next_halfedge_handle(heh);
    vh[1] = mesh().to_vertex_handle(heh);
    uvw[1] = mesh().property(vprop_param_, vh[1]);
    pos[1] = mesh().point(vh[1]);
    heh = mesh().next_halfedge_handle(heh);
    vh[2] = mesh().to_vertex_handle(heh);
    uvw[2] = mesh().property(vprop_param_, vh[2]);
    pos[2] = mesh().point(vh[2]);
    heh = mesh().next_halfedge_handle(mesh().opposite_halfedge_handle(heh));
    vh[3] = mesh().to_vertex_handle(heh);
    uvw[3] = mesh().property(vprop_param_, vh[3]);
    pos[3] = mesh().point(vh[3]);
    
    sdv  = OpenVolMesh::dot(pos[3]-pos[0], OpenVolMesh::cross(pos[1]-pos[0], pos[2]-pos[0])) / 6.0f;
    sdpv = OpenVolMesh::dot(uvw[3]-uvw[0], OpenVolMesh::cross(uvw[1]-uvw[0], uvw[2]-uvw[0])) / 6.0f;

    if (sdv <= 0)
    {
      std::cout << "geometric flipping in ch: " << cc_it << "   " << "sdpv: " << sdpv << std::endl;
      std::cout << "     [0]  pos: " << pos[0] << "     uvw: " << uvw[0] << "    is_boundary: " << (mesh().is_boundary(vh[0]) ? "yes" : "no") << std::endl;
      std::cout << "     [1]  pos: " << pos[1] << "     uvw: " << uvw[1] << "    is_boundary: " << (mesh().is_boundary(vh[1]) ? "yes" : "no") << std::endl;
      std::cout << "     [2]  pos: " << pos[2] << "     uvw: " << uvw[2] << "    is_boundary: " << (mesh().is_boundary(vh[2]) ? "yes" : "no") << std::endl;
      std::cout << "     [3]  pos: " << pos[3] << "     uvw: " << uvw[3] << "    is_boundary: " << (mesh().is_boundary(vh[3]) ? "yes" : "no") << std::endl;
    }

    if (sdpv <= 0)
    {
      std::cout << "parameteric flipping in ch: " << cc_it << "   " << "sdpv: " << sdpv << std::endl;
      std::cout << "     [0]  pos: " << pos[0] << "     uvw: " << uvw[0] << "    is_boundary: " << (mesh().is_boundary(vh[0]) ? "yes" : "no") << std::endl;
      std::cout << "     [1]  pos: " << pos[1] << "     uvw: " << uvw[1] << "    is_boundary: " << (mesh().is_boundary(vh[1]) ? "yes" : "no") << std::endl;
      std::cout << "     [2]  pos: " << pos[2] << "     uvw: " << uvw[2] << "    is_boundary: " << (mesh().is_boundary(vh[2]) ? "yes" : "no") << std::endl;
      std::cout << "     [3]  pos: " << pos[3] << "     uvw: " << uvw[3] << "    is_boundary: " << (mesh().is_boundary(vh[3]) ? "yes" : "no") << std::endl;
      
    }
    else
    {
      std::cout << cc_it << "-th cell is OK (no flipping)! sdpv: " << sdpv << std::endl;
    }
  }  
	std::cout << "**************************************" << std::endl;
    
}


void FnVolumeHarmonicParam::save(const std::string& _filename) const
{
  FILE    *fp;
  Mesh::ConstVertexIter         cv_it;
  Mesh::ConstFaceIter           cf_it;
  Mesh::ConstCellIter           cc_it;
  Mesh::ConstFaceVertexIter     cfv_it;
  Mesh::ConstCellIHalffaceIter  ccihf_it;

  fp = fopen(_filename.c_str(), "w");

  fprintf(fp, "# %d vertices\n", mesh().n_vertices());
  fprintf(fp, "# %d faces\n", mesh().n_faces());
  fprintf(fp, "# %d cells\n", mesh().n_cells());
  fprintf(fp, "\n");
  
  // v
  for (cv_it=mesh().vertices_begin(); cv_it!=mesh().vertices_end(); ++cv_it)
  {
    Mesh::Point p = mesh().point(cv_it);
    fprintf(fp, "v %f %f %f\n", (Scalar) p[0], (Scalar) p[1], (Scalar) p[2]);
  }

  // vt
  for (cv_it=mesh().vertices_begin(); cv_it!=mesh().vertices_end(); ++cv_it)
  {
    Param uvw = mesh().property(vprop_param_, cv_it);
    fprintf(fp, "vt %f %f %f\n", (Scalar) uvw[0], (Scalar) uvw[1], (Scalar) uvw[2]);
  }

  // f
  for (cf_it=mesh().faces_begin(); cf_it!=mesh().faces_end(); ++cf_it)
  {
    fprintf(fp, "f");
    for (cfv_it=mesh().cfv_iter(cf_it); cfv_it; ++cfv_it)
      fprintf(fp, " %d/%d", 1+cfv_it.handle().idx(), 1+cfv_it.handle().idx());
    fprintf(fp, "\n");
  }

  // c
  for (cc_it=mesh().cells_begin(); cc_it!=mesh().cells_end(); ++cc_it)
  {
    fprintf(fp, "c");
    for (ccihf_it=mesh().ccihf_iter(cc_it); ccihf_it; ++ccihf_it)
    {
      int idx = ccihf_it.handle().idx();
      fprintf(fp, " %d", ((idx%2 == 0) ? (1+idx/2) : -(1+idx/2)));
    }
    fprintf(fp, "\n");
  }

  fclose(fp);
}


void FnVolumeHarmonicParam::export_linear_system(const std::string& _A_dat, const std::string& _b_dat)
{
  FILE  *fp_A, *fp_B;
  int     n;
  int     i, j;
  Scalar  w_ii, w_ij;
  Scalar  b_0, b_1;    

  Mesh::VertexIter      v_it;
  Mesh::VertexEdgeIter  ve_it;
  Mesh::VertexHandle    vh_j;


  fp_A = fopen(_A_dat.c_str(), "w");
  fp_B = fopen(_b_dat.c_str(), "w");
  
  n = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    if (mesh().property(vprop_lns_idx_, v_it) < 0)
      continue;
    ++n;
  }

  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)  
  {
    i = mesh().property(vprop_lns_idx_, v_it);
    if (i < 0)
      continue;

    w_ii = 0;
    b_0 = 0;
    b_1 = 0;
    for (ve_it=mesh().ve_iter(v_it); ve_it; ++ve_it)
    {
      w_ij = mesh().property(eprop_volume_w_, ve_it);
      w_ii += w_ij;
            
      vh_j = mesh().to_vertex_handle(mesh().halfedge_handle(ve_it));
      if (vh_j == v_it)
        vh_j = mesh().from_vertex_handle(mesh().halfedge_handle(ve_it));

      j = mesh().property(vprop_lns_idx_, vh_j);
      if (j >= 0)           // set element of A
      {
        fprintf(fp_A, "%d %d %f\n", 1+i, 1+j, -w_ij);
      }
      else                  // set element of B
      {
        Param uv = mesh().property(vprop_param_, vh_j);

        b_0 += w_ij*uv[0];
        b_1 += w_ij*uv[1]; 
      }
    }
    fprintf(fp_A, "%d %d %f\n", 1+i, 1+i, w_ii);
    fprintf(fp_B, "%d 1 %f\n", 1+i, b_0);
    fprintf(fp_B, "%d 2 %f\n", 1+i, b_1);
  }
  fclose(fp_A);
  fclose(fp_B);
}




int FnVolumeHarmonicParam::calc_with_taucs()
{
  taucs_ccs_matrix  *A = NULL;
  taucs_double      *u, *v, *w;
  taucs_double      *bu, *bv, *bw;
  FILE*             fp_ijv;
  int               i, j, num_nz;
  int               n;
  int               rc;
  char*             ijv_filename = "taucs_matrix.ijv";

  printf("FnVolumeHarmonicParam::calc_with_taucs()\n");

  Mesh::VertexIter            v_it;
  Mesh::EdgeIter              e_it;
  Mesh::VertexEdgeIter        ve_it;
  Mesh::HalfedgeHandle        heh[2];
  Mesh::VertexHandle          vh_j;

  // calc # of free vertices (i.e., n x n)
  n = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)
  {
    if (mesh().property(vprop_lns_idx_, v_it) < 0)
      continue;
    ++n;
  }
  printf("n=%d\n", n);
   

  // fill the matrix A and bu, bv, bw;
  fp_ijv = fopen(ijv_filename, "w");
  bu = (taucs_double *) taucs_vec_create(n, TAUCS_DOUBLE);
  bv = (taucs_double *) taucs_vec_create(n, TAUCS_DOUBLE);
  bw = (taucs_double *) taucs_vec_create(n, TAUCS_DOUBLE);
  num_nz = 0;
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)  
  {
    double  w_ii, w_ij;

    i = mesh().property(vprop_lns_idx_, v_it);
    if (i < 0)
      continue;

    w_ii = 0;
    bu[i] = 0;
    bv[i] = 0;
    bw[i] = 0;

    for (ve_it=mesh().ve_iter(v_it); ve_it; ++ve_it)
    {
      w_ij = mesh().property(eprop_volume_w_, ve_it);
      w_ii += w_ij;

      vh_j = mesh().to_vertex_handle(mesh().halfedge_handle(ve_it));
      if (vh_j == v_it)
        vh_j = mesh().from_vertex_handle(mesh().halfedge_handle(ve_it));

      j = mesh().property(vprop_lns_idx_, vh_j);
      if (j >= 0)         // set element of A
      {
        fprintf(fp_ijv, "%d %d %f\n", i+1, j+1, (float) -w_ij);
        ++num_nz;
      }
      else                // set element of B
      {
        Param uvw = mesh().property(vprop_param_, vh_j);
        
        bu[i] += w_ij*uvw[0];
        bv[i] += w_ij*uvw[1];
        bw[i] += w_ij*uvw[2];
      }
    }
    fprintf(fp_ijv, "%d %d %f\n", i+1, i+1, (float) w_ii);
    ++num_nz;
  }
  fclose(fp_ijv);

  taucs_logfile("stdout");


  A = taucs_ccs_read_ijv(ijv_filename, TAUCS_SYMMETRIC | TAUCS_DOUBLE);
  u = (taucs_double *) taucs_vec_create(A->n, A->flags);
  v = (taucs_double *) taucs_vec_create(A->n, A->flags);
  w = (taucs_double *) taucs_vec_create(A->n, A->flags);
  for (i=0; i<n; ++i)   { u[i] = v[i] = w[i] = 0; }

  taucs_ccs_write_ijv(A, "test.ijv");     // just for debugging




  /////////////////////////////////////
  //// cg solver (it is working also!)
  /////////////////////////////////////
  //printf("cg solver\n");
  //
  //for (i=0; i<n; ++i)   { printf("bu[%3d] = %lf\n", i, bu[i]); }
  //for (i=0; i<n; ++i)   { printf("bv[%3d] = %lf\n", i, bv[i]); }
  //for (i=0; i<n; ++i)   { printf("bw[%3d] = %lf\n", i, bw[i]); }
  //
  //// computing u
  //rc = taucs_conjugate_gradients(A, NULL, NULL, u, bu, 2*A->n, 1e-7);
  //if (rc == TAUCS_SUCCESS)  printf("cg solver for computing u: it's ok\n");
  //else                      printf("cg solver for computing u: it's NOT ok\n");
  //// computing v
  //rc = taucs_conjugate_gradients(A, NULL, NULL, v, bv, 2*A->n, 1e-7);
  //if (rc == TAUCS_SUCCESS)  printf("cg solver for computing v: it's ok\n");
  //else                      printf("cg solver for computing v: it's NOT ok\n");
  //// computing w
  //rc = taucs_conjugate_gradients(A, NULL, NULL, w, bw, 2*A->n, 1e-7);
  //if (rc == TAUCS_SUCCESS)  printf("cg solver for computing w: it's ok\n");
  //else                      printf("cg solver for computing w: it's NOT ok\n");


  //////////////////////////////////////////////////////////////////////////
  //// LLT factor, solve, free the factorization
  //////////////////////////////////////////////////////////////////////////
  char* options_factor[] = {"taucs.factor.LLT=true", NULL };
  void* F = NULL;
  rc = taucs_linsolve(A, &F, 0, NULL, NULL, options_factor, NULL);    // factor
  if (rc != TAUCS_SUCCESS)  printf("problem in factor\n");
  rc = taucs_linsolve(A, &F, 1, u, bu, options_factor, NULL);        // computing u
  if (rc != TAUCS_SUCCESS)  printf("problem in solving x0\n");
  rc = taucs_linsolve(A, &F, 1, v, bv, options_factor, NULL);        // computing v
  if (rc != TAUCS_SUCCESS)  printf("problem in solving x1\n");
  rc = taucs_linsolve(A, &F, 1, w, bw, options_factor, NULL);        // computing w
  if (rc != TAUCS_SUCCESS)  printf("problem in solving x1\n");
  rc = taucs_linsolve(NULL, &F, 0, NULL, NULL, NULL, NULL);
  if (rc != TAUCS_SUCCESS)  printf("problem in free\n");


  // get results
  for (v_it=mesh().vertices_begin(); v_it!=mesh().vertices_end(); ++v_it)  
  {
    i = mesh().property(vprop_lns_idx_, v_it);
    if (i < 0)
      continue;

    Param   uvw;

    uvw[0] = (Scalar) u[i];
    uvw[1] = (Scalar) v[i];
    uvw[2] = (Scalar) w[i];

    mesh().property(vprop_param_, v_it) = uvw;
  }
  

  return  0;
}